#include <stdio.h>
#include <ulog/ulog.h>
#include "iic_function.h"
#include "camera_config.h"
#include "drv_isp.h"
#include "soc.h"
#include "pin_name.h"
#include "sc031gs_drv.h"
#include "sc031gs_config.h"
#include "drv_gpio.h"
#include "cli_api.h"

static i2c_dev_t i2c_sc031gs;
static i2c_dev_t i2c_sc031gs_right;

extern gpio_pin_handle_t gpio_pin_handle;

regval_list sc031_config_reg[] = {
	{0x0000 , 0x00},
};

static int sc031gs_check_id(i2c_dev_t *i2c)
{
    int ret = 0;
    uint8_t data[2] = {0};

	//printf("check id , adr %08x \n", i2c->config.dev_addr);
	ret = iic_read16bit(i2c,  SC031GS_ADDR_CHIPID_H, data);
	printf("sc031gs: chipID H = 0x%02x \n", data[0]);
	if (ret < 0 || (data[0] != SC031GS_DATA_CHIPID_H))
		return -1;

	ret = iic_read16bit(i2c,  SC031GS_ADDR_CHIPID_L, data);
	printf("sc031gs: chipID L = 0x%02x \n", data[0]);
	
   	if (ret < 0 || (data[0] != SC031GS_DATA_CHIPID_L))
        return -1;

    return ret;
}


static void camera_io_out32(uint32_t addr, uint32_t value)			//write
{
	volatile uint32_t *ptr = (volatile uint32_t *)(addr);
	//printf("write: addr=0x%x, data=0x%x\n", addr, value);
	*ptr = value;
}

static uint32_t camera_io_in32(uint32_t addr)						//read
{
	volatile uint32_t *ptr = (volatile uint32_t *)(addr);
	//printf("read:  addr=0x%x, data=0x%x\n", ptr, *ptr);
	
	return *ptr;
}

static void camera_io_rstn_init(uint8_t index)
{
	uint32_t reg_val;

	reg_val = camera_io_in32(CSKY_AOGPIO_BASE + 0x04);
	reg_val |= (0x1 << index);
	
	camera_io_out32(CSKY_AOGPIO_BASE + 0x04, reg_val);
}

void camera_io_rstn_set(uint8_t index, bool level)
{
	uint32_t reg_val;

	reg_val = camera_io_in32(CSKY_AOGPIO_BASE + 0x00); 
	if (level)
		reg_val |= (0x1 << index);
	else
		reg_val &= ~(0x1 << index);

	camera_io_out32(CSKY_AOGPIO_BASE + 0x00, reg_val);
}

void sensorHwReset(int sensor_idx){
    camera_io_rstn_init(sensor_idx);
    camera_io_rstn_set(sensor_idx, true);    aos_msleep(1);
    camera_io_rstn_set(sensor_idx, false);   aos_msleep(1);
    camera_io_rstn_set(sensor_idx, true);    aos_msleep(1);
}


int sc031gs_hw_reset(void)
{
	volatile uint32_t *base;

    sensorHwReset(7);
#if 0
	//IR_PWRDN UART1_RTSN GPIOA[21]
    base = (uint32_t *)(CSKY_GPIO0_BASE + 0x04);  //GPIOA direction
    *base |= (0x1 << 21); 
    base = (uint32_t *)(CSKY_GPIO0_BASE + 0x00);  //out
    *base |= (0x1 << 21); //IR_PWRDN

	//IR_RESET AOGPIO7
	base = (uint32_t *)(CSKY_AOGPIO_BASE + 0x04); //gpio dicrection
	*base |= (0x1 << 7);
	base = (uint32_t *)(CSKY_AOGPIO_BASE + 0x00); //output
    *base &= ~(0x1 << 7);
    aos_msleep(1);
	*base |= (0x1 << 7);
#endif 
    return 0;
}

int sc031_write1(uint16_t reg, uint8_t value){

    int ret = 0;
    sc031_config_reg[0].addr = reg;
    sc031_config_reg[0].data = value;
//    ret = iic_write(&sc031_config_reg, (struct regval_list *)sc031_config_reg, ARRAY_SIZE(sc031_config_reg));
    ret = iic_write(&i2c_sc031gs_right, reg, value);

}

int sc031_read1(uint16_t reg){

    int ret = 0;
    uint8_t data[2] = {0};

    ret = iic_read(&i2c_sc031gs_right,  reg, &data);

    printf("read reg 0x%02x value 0x%x \n", reg,data[0]);
}


int sc031gs_init(i2c_dev_t *i2c, uint32_t dev_addr)
{
   	memcpy(&i2c_sc031gs, i2c, sizeof(i2c_dev_t));
    i2c_sc031gs.config.dev_addr = dev_addr;
    //Check the id of camera
   	int ret = sc031gs_check_id(&i2c_sc031gs);
    if (ret < 0) {
        printf("sc031gs check id fail\n");
        return ret;
    }
	
    return ret;
}

int sc031gs_right_init(i2c_dev_t *i2c, uint32_t dev_addr)
{
	memcpy(&i2c_sc031gs_right, i2c, sizeof(i2c_dev_t));
    i2c_sc031gs_right.config.dev_addr = dev_addr;
    //Check the id of camera
   	int ret = sc031gs_check_id(&i2c_sc031gs_right);
    if (ret < 0) {
        printf("sc031gs check id fail\n");
        return ret;
    }
	
    return ret;
}

int32_t sc031gs_mode_config(sensor_mode_t mode)
{
	int ret = 0;
	//printf("%s >>>>  %d, mode %d \n",__func__, __LINE__, mode);

	if (STANDBY == mode) {
		ret = iic_write_array(&i2c_sc031gs, (struct regval_list *)sc031gs_sw_standby, ARRAY_SIZE(sc031gs_sw_standby));
		if (ret < 0) {
			return -1;
		}
	} 
	else if(STREAM == mode)
	{
		ret = iic_write_array(&i2c_sc031gs, (struct regval_list *)sc031gs_streaming, ARRAY_SIZE(sc031gs_streaming));
		if (ret < 0) {
			return -1;
		}
	}
	else {}

	return 0;
}


int sc031gs_right_mode_config(sensor_mode_t mode)
{
	int ret = 0;
	//printf("%s >>>>  %d, mode %d \n",__func__, __LINE__, mode);

	if (STANDBY == mode) {
		ret = iic_write_array(&i2c_sc031gs_right, (struct regval_list *)sc031gs_sw_standby, ARRAY_SIZE(sc031gs_sw_standby));
		if (ret < 0) {
			return -1;
		}
	} 
	else if(STREAM == mode)
	{
		ret = iic_write_array(&i2c_sc031gs_right, (struct regval_list *)sc031gs_streaming, ARRAY_SIZE(sc031gs_streaming));
		if (ret < 0) {
			return -1;
		}
	}
	else {}

	return 0;
}

int32_t sc031gs_config(imageFormat_t cfg)
{
	int32_t ret = 0;

	//printf("sc031gs config. \n");

	if( R_640x480 == cfg.res && RAW10 == cfg.format)
	{
		ret = iic_write_array(&i2c_sc031gs, (struct regval_list *)sc031gs_640x480_raw10, ARRAY_SIZE(sc031gs_640x480_raw10));
		if (ret < 0) {
			return -1;
		}

		// ret = iic_read_array(&i2c_sc031gs, (struct regval_list *)sc031gs_640x480_raw8, ARRAY_SIZE(sc031gs_640x480_raw8));
		// if (ret < 0) {
		// 	return -1;
		// }
		
		//sc031gs_mode_config(SC031GS_STANDBY);
	
		//printf("sc031gs reg init done. \n");
	}
	else if( R_640x480 == cfg.res && RAW8 == cfg.format)
	{
		ret = iic_write_array(&i2c_sc031gs, (struct regval_list *)sc031gs_640x480_raw8, ARRAY_SIZE(sc031gs_640x480_raw8));
		if (ret < 0) {
			return -1;
		}

		// ret = iic_read_array(&i2c_sc031gs, (struct regval_list *)sc031gs_640x480_raw8, ARRAY_SIZE(sc031gs_640x480_raw8));
		// if (ret < 0) {
		// 	return -1;
		// }
		
		//sc031gs_mode_config(SC031GS_STANDBY);
	
		//printf("sc031gs reg init done. \n");
	}

	return ret;
}

int32_t sc031gs_right_config(imageFormat_t cfg)
{
	int ret = 0;
	
	return ret;
}


int sc031gs_debug_test(void)
{
	// iic_write_array(&i2c_sc031gs, (struct regval_list *)sc031gs_640x480_raw8, ARRAY_SIZE(sc031gs_640x480_raw8));
	// aos_msleep(100);
	// iic_read_array(&i2c_sc031gs, (struct regval_list *)sc031gs_640x480_raw8, ARRAY_SIZE(sc031gs_640x480_raw8));

	//iic_write_array(&i2c_sc031gs, (struct regval_list *)sc031gs_debug_ae, ARRAY_SIZE(sc031gs_debug_ae));
	aos_msleep(100);
	iic_read_array(&i2c_sc031gs, (struct regval_list *)sc031gs_640x480_raw8, ARRAY_SIZE(sc031gs_640x480_raw8));

	return 0;
}

int sc031_write(uint8_t isp_idx, uint16_t reg, uint8_t value){

    int ret = 0;
    sc031_config_reg[0].addr = reg;
    sc031_config_reg[0].data = value;
//    ret = iic_write(&sc031_config_reg, (struct regval_list *)sc031_config_reg, ARRAY_SIZE(sc031_config_reg));

    if(isp_idx == 0)
        ret = iic_write(&i2c_sc031gs_right, reg, value);
    if(isp_idx == 1)
        ret = iic_write(&i2c_sc031gs, reg, value);

}

int sc031_read(uint8_t isp_idx,uint16_t reg){

    int ret = 0;
    uint8_t data[2] = {0};

    if(isp_idx == 0)
        ret = iic_read(&i2c_sc031gs_right,  reg, &data);
    if(isp_idx == 1)
        ret = iic_read(&i2c_sc031gs,  reg, &data);

    printf("read reg 0x%02x value 0x%x \n", reg,data[0]);
}


void sensor_command(char *outbuf, int len, int argc, char **argv)
{
    uint16_t reg = 0;
    uint8_t value = 0;
    uint8_t isp_idx = 0;
    
    if (argc == 1) {
        printf("wrong argv numer.\n");
        return;
    }
    if (!strcmp(argv[1], "set")) {
        isp_idx = strtoul(argv[2], NULL, 16);
        reg = strtoul(argv[3], NULL, 16);
        value = strtoul(argv[4], NULL, 16);
        printf("set sensor%d reg 0x%x, value 0x%x ",isp_idx, reg, value);
        sc031_write(isp_idx, reg,value);
        
    } 
    else if (!strcmp(argv[1], "get")) {
        
            isp_idx = strtoul(argv[2], NULL, 16);
            reg = strtoul(argv[3], NULL, 16);
            printf("get sensor%d reg 0x%x \n",isp_idx, reg);
            sc031_read(isp_idx, reg);
        }
    else if (!strcmp(argv[1], "set1")) {
        reg = strtoul(argv[2], NULL, 16);
        value = strtoul(argv[3], NULL, 16);
        printf("set reg 0x%x, value 0x%x ", reg, value);
        sc031_write1(reg, value);
        
    } 
    else if (!strcmp(argv[1], "get1")) {
        
            reg = strtoul(argv[2], NULL, 16);
            
            printf("get reg 0x%x \n", reg);
            sc031_read1(reg);
        }
    else {
        cli_printf("param invalid!\n");
        return;
    }

    return;
}




static const struct cli_command_st img_cli_cmd[] = 
{
    [0] =
    {
        .name            = "sensor",
        .help            = "sensor set/get register",
        .function        = sensor_command
    },

};


int img_cli_register(void) 
{
    int ret = 0;

    ret = cli_register_commands(img_cli_cmd, sizeof(img_cli_cmd)/sizeof(struct cli_command_st));
    if (ret != 0) {
        return ret;
    }

    return 0;
}

void sc031gs_reg_test()
{
	iic_read_array(&i2c_sc031gs, (struct regval_list *)sc031gs_debug, ARRAY_SIZE(sc031gs_debug));
}

